#include <robot/sdh/cvirtualhand.h>

#include <buola/io/curi.h>

#include <buola/scene/transform/crttransform.h>
#include <buola/scene/cgeode.h>

#define ROBOT_SHARE_DIR "/usr/local/share/robot"

namespace buola { namespace robot { namespace sdh {

CVirtualHand::CVirtualHand()
{
    mGroup=new scene::CRTTransform;

    scene::PGeode lGeode;
    scene::PRTTransform lTranslation,lRotation;

    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_base.iv",0.001);
    lGeode->AttachTo(mGroup);
    
    lRotation=new scene::CRTTransform;
    lRotation->SetRotation(mat::CQuat_d(M_PI_2,mat::CVec3d({1,0,0})));
    mGroup->AddChild(lRotation);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(-0.0381053,0,0.0232));
    lRotation->AddChild(lTranslation);
    lRotation=new scene::CRTTransform;
    lRotation->SetRotation(mat::CQuat_d(-M_PI_2,mat::CVec3d({0,0,1})));
    lTranslation->AddChild(lRotation);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_knuckle.iv",0.001);
    lGeode->AttachTo(lRotation);

    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(0.00634,0,0.0175));
    lRotation->AddChild(lTranslation);
    mRotations[3]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[3]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_middle.iv",0.001);
    lGeode->AttachTo(mRotations[3]);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(-0.0207,0,0.0865));
    mRotations[3]->AddChild(lTranslation);
    mRotations[4]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[4]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_finger.iv",0.001);
    lGeode->AttachTo(mRotations[4]);

    lRotation=new scene::CRTTransform;
    lRotation->SetRotation(mat::CQuat_d(M_PI_2,mat::CVec3d(1,0,0)));
    mGroup->AddChild(lRotation);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(0.019053,0.033,0.0232));
    lRotation->AddChild(lTranslation);
    mRotations[0]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[0]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_knuckle.iv",0.001);
    lGeode->AttachTo(mRotations[0]);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(0.00634,0,0.0175));
    mRotations[0]->AddChild(lTranslation);
    mRotations[5]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[5]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_middle.iv",0.001);
    lGeode->AttachTo(mRotations[5]);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(-0.0207,0,0.0865));
    mRotations[5]->AddChild(lTranslation);
    mRotations[6]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[6]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_finger.iv",0.001);
    lGeode->AttachTo(mRotations[6]);

    lRotation=new scene::CRTTransform;
    lRotation->SetRotation(mat::CQuat_d(M_PI_2,mat::CVec3d(1,0,0)));
    mGroup->AddChild(lRotation);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(0.019053,-0.033,0.0232));
    lRotation->AddChild(lTranslation);
    mRotations[7]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[7]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_knuckle.iv",0.001);
    lGeode->AttachTo(mRotations[7]);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(0.00634,0,0.0175));
    mRotations[7]->AddChild(lTranslation);
    mRotations[1]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[1]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_middle.iv",0.001);
    lGeode->AttachTo(mRotations[1]);
    lTranslation=new scene::CRTTransform;
    lTranslation->SetTranslation(mat::CVec3d(-0.0207,0,0.0865));
    mRotations[1]->AddChild(lTranslation);
    mRotations[2]=new scene::CRTTransform;
    lTranslation->AddChild(mRotations[2]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/SCHUNKHAND/schunkhand_finger.iv",0.001);
    lGeode->AttachTo(mRotations[2]);

    for(int i=0;i<7;i++) mPose[i]=0;
}

CVirtualHand::~CVirtualHand()
{
}

void CVirtualHand::SetPose(const mat::CVecNd<7> &pPose)
{
    lock_guard<mutex> lLock(mMutex);
    mPose=pPose;
}

mat::CVecNd<7> CVirtualHand::GetPose()
{
    lock_guard<mutex> lLock(mMutex);
    return mPose;
}

void CVirtualHand::Update()
{
    lock_guard<mutex> lLock(mMutex);
    mRotations[0]->SetRotation(mat::CQuat_d(M_PI_2+mPose[0],mat::CVec3d(0,0,1)));
    mRotations[7]->SetRotation(mat::CQuat_d(M_PI_2-mPose[0],mat::CVec3d(0,0,1)));
    mRotations[1]->SetRotation(mat::CQuat_d(-mPose[1],mat::CVec3d(1,0,0)));
    mRotations[2]->SetRotation(mat::CQuat_d(-mPose[2],mat::CVec3d(1,0,0)));
    mRotations[3]->SetRotation(mat::CQuat_d(-mPose[3],mat::CVec3d(1,0,0)));
    mRotations[4]->SetRotation(mat::CQuat_d(-mPose[4],mat::CVec3d(1,0,0)));
    mRotations[5]->SetRotation(mat::CQuat_d(-mPose[5],mat::CVec3d(1,0,0)));
    mRotations[6]->SetRotation(mat::CQuat_d(-mPose[6],mat::CVec3d(1,0,0)));
}

/*namespace sdh*/ } /*namespace robot*/ } /*namespace buola*/ }
